#include "NXTwc.h"

CvCapture* capture = 0;

int WCRunning(int device_n){
	/* Se activa la webcam */
	capture = cvCaptureFromCAM(device_n);
	if (capture == 0) return -1;
	else return device_n;
}

int WCSearching(int minred, int mingreen, int minblue, int maxred, int maxgreen, int maxblue, double* area){
	IplImage* frame = 0;
	IplImage* imgHSV = 0;
	IplImage* imgThresh = 0;
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
	double posX, area_aux;
	double moment10, moment01;

	/* Se captura el fotograma de la webcam */
	frame = cvQueryFrame(capture);
	if (frame == 0) return -1;

	/* Se pasa de BGR a HSV el fotograma */
	imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
	cvCvtColor(frame, imgHSV, CV_BGR2HSV);

	/* Se obtiene la imagen filtrada */
	imgThresh = cvCreateImage(cvGetSize(imgHSV), IPL_DEPTH_8U, 1);
	cvInRangeS(imgHSV, cvScalar(minblue,mingreen,minred,1), cvScalar(maxblue,maxgreen,maxred,1), imgThresh);

	/* Se calculan los momentos espaciales para obtener el área y el centro */
	cvMoments(imgThresh, moments, 1);
	moment10 = cvGetSpatialMoment(moments, 1, 0);
	area_aux = cvGetCentralMoment(moments, 0, 0);

	if (area_aux > 0){
		posX = moment10/area_aux;
		*area = area_aux;
	}
	else {
		posX = -1;
		*area = -1;
	}
	free(moments);
	
	return posX;
}

void WCThresholding(int minred, int mingreen, int minblue, int maxred, int maxgreen, int maxblue){
	int posX, posY, c;
	IplImage* frame=0;
	IplImage* imgThresh;
	IplImage* imgHSV;
	double moment10, moment01, area; 
	CvMoments *moments;

	/* Se crean las ventanas */	
	cvNamedWindow("Ball",1);     
	cvNamedWindow("HSV",1);
	cvNamedWindow("Video",1);

	while(1){
		/* Se captura el fotograma de la webcam */
		frame = cvQueryFrame(capture);           
		if(!frame) break;
		frame=cvCloneImage(frame); 
		
		/* Se clona la imagen de la webcam y se cambia de BGR a HSV */
		imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3); 
		cvCvtColor(frame, imgHSV, CV_BGR2HSV);

		/* Se obtiene la imagen filtrada*/
		imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
		cvInRangeS(imgHSV, cvScalar(minblue,mingreen,minred,1), cvScalar(maxblue,maxgreen,maxred,1), imgThresh); 		

		/* Se calculan los momentos espaciales para obtener el área y el centro */
		moments = (CvMoments*)malloc(sizeof(CvMoments));
		cvMoments(imgThresh, moments, 1);
		moment10 = cvGetSpatialMoment(moments, 1, 0);
		moment01 = cvGetSpatialMoment(moments, 0, 1);
		area = cvGetCentralMoment(moments, 0, 0);
		
		if(area>0){
			posX = moment10/area;
			posY = moment01/area;
		}
		fprintf(stderr, "Area: %f   PositionX: %i   PositionY: %i\n", area, posX, posY);
		free(moments);		
		
		/* Se muestra la imagen filtrada, con el cambio a HSV y la original */
		cvShowImage("Ball", imgThresh);           
		cvShowImage("HSV", imgHSV);
		cvShowImage("Video", frame);
		
		/* Si se pulsa 'Esc' se sale del bucle y guardamos las 3 últimas imágenes*/
		c = cvWaitKey(10);
		if((char)c==27) {
			cvSaveImage("Img_hsv.ppm", imgHSV, 0);
			cvSaveImage("Img_bin.ppm", imgThresh, 0);
			cvSaveImage("Img_bgr.ppm", frame, 0);
			break;
		}
	}
	cvDestroyAllWindows() ;
}